#include "code.h"
#include <numeric>
#include <algorithm>
#include <iostream>
#include <cstdlib>
#include <math.h>
#define P 10000
#define random() rand()%(P)/(float)(P)
#define randomint(a,b) (rand() % (b-a))+ a; 


void swap(code* DNA);
code TSPSA(code DNA, city* C, double _t0, int Nl, int Nt);


void swap(code* DNA){
  int *arr=DNA->get_array();
  int l=DNA->get_length();
  int p=0,q=0;
  while (p == q){
     p=randomint(0,l);
     q=randomint(0,l);
  }
  if (p>q){
    int tmp = p;
    p = q;
    q = tmp;
  }
  int *narr= new int[l];
  for (int i=0; i<l ; i++){
    if (i<p || i>q)
      narr[i]=arr[i];
    else
      narr[i]=arr[p+q-i];
  }
  DNA->set_array(narr);
}

code TSPSA(code DNA, city* C, double t0, int Nl, int Nt){
  int k=0;
  double t=t0;
  int l=DNA.get_length();
  code best_DNA=DNA;
  double best_d=best_DNA.total_dis(C,l);
  double d,nd,h,U;
  code sDNA;
  for (int i=0; i<Nt ; i++){
    while (k<Nl){
      d=DNA.total_dis(C,l);
      if (d<best_d){
	best_DNA=DNA;
	best_d=d;
      }
      sDNA=DNA;
      swap(&DNA);
      nd=DNA.total_dis(C,l);
      h=exp(-(nd - d)/t);
      if (h>1)
	h=1;
      U=random();
      if (U>h)
	DNA=sDNA;
      k++;
    }
    std::cout << "t:" << t << std::endl;
    std::cout << "d:" << d << std::endl;
    t = t * (1 - 1 / (50 + log(i+1)));
    k=0;
  }
  return best_DNA;
}
